import java.util.Vector;

public class MySuperRobot {
	/*
	public static enum Dir {NORTH, EAST, SOUTH, WEST};
	public static enum GridState {TOUCHED, UNTOUCHED};
	public static enum WallState {UNVISITED, NOWALL, VISITED};
	*/
	public static final byte NORTH = 0, EAST = 1, SOUTH = 2, WEST = 3, NODIR = 5;
	// for readability sake
	public static final byte[] GRIDXOFFSET = {0, 1, 0, -1};
	public static final byte[] GRIDYOFFSET = {-1, 0, 1, 0};
	public static final byte[] WALLSOFFSET = {-1, 0, 0, 0};
	public static final byte[] WALLEOFFSET = {0, 0, -1, 0};

	public static final byte UNTOUCHED = 0, TOUCHED = 1, UNVISITED = 0, NOWALL = 1, VISITED = 3;

	public static final byte
		MAZEWIDTH = 4,
							STARTDIR = SOUTH,
							STARTX = 0, 
							STARTY = 0,
							GOALX = 3,
							GOALY = 1;

	public byte[][] grid, wallS, wallE;
	public byte currDir, currX, currY, goalX, goalY, tempD;
	public int countUnvisited;

	boolean[][] bfsvisited; // for nextDir
	// LinkedList<bfsState> q = new LinkedList<bfsState>();
	Vector q = new Vector();

	// DummyMove rover = new DummyMove();
	Move rover = new Move();

	MySuperRobot() {
		grid = new byte[MAZEWIDTH][MAZEWIDTH];
		wallS = new byte[MAZEWIDTH][MAZEWIDTH];
		wallE = new byte[MAZEWIDTH][MAZEWIDTH];

		bfsvisited = new boolean[MAZEWIDTH][MAZEWIDTH];

		for (int i = 0; i < MAZEWIDTH; i++) {
			for (int j = 0; j < MAZEWIDTH; j++) {
				grid[i][j] = UNTOUCHED;
				if (i != MAZEWIDTH-1) {
					wallS[j][i] = UNVISITED;
					wallE[i][j] = UNVISITED;
				} else {
					wallS[j][i] = VISITED; // bottom row has a wall at south
					wallE[i][j] = VISITED; // right-most col has a wall at east
				}
			}
		}

		// initialise start position
		currX = STARTX;
		currY = STARTY;
		goalX = GOALX;
		goalY = GOALY;
		currDir = STARTDIR;
	}

	public void move() 
		throws InterruptedException
	{
		boolean moved = true, goalFound = false;
		// or true?

		while(moved) {
			moved = false;

			if (currX == goalX && currY == goalY) {
				goalFound = true;
				break; // WHOOHOO
			}
/*
			// TODO: For testing
			rover.printMaze();
				System.out.println();
			for (int i = 0; i < MAZEWIDTH; i++) {
				for (int j = 0; j < MAZEWIDTH; j++) {
					System.out.print(grid[j][i]);
				}
				System.out.println();
			}
			for (int i = 0; i < MAZEWIDTH; i++) {
				for (int j = 0; j < MAZEWIDTH; j++) {
					System.out.print(wallS[j][i]);
				}
				System.out.println();
			}

			for (int i = 0; i < MAZEWIDTH; i++) {
				for (int j = 0; j < MAZEWIDTH; j++) {
					System.out.print(wallE[j][i]);
				}
				System.out.println();
			}
			System.out.println("curr : " + currX + ", " + currY + " " + currDir);
*/
			// check around for visited
			// NORTH
			if (currY > 0 &&
					wallS[currX][currY - 1] == UNVISITED)
			{
				if ( grid[currX][currY - 1] == TOUCHED ) {
					countUnvisited--;
					wallS[currX][currY - 1] = VISITED;
				} else if (grid[currX][currY] == UNTOUCHED ) 
					countUnvisited++; // add unvisited first time coming here
			}

			// EAST
			if (currX < MAZEWIDTH - 1 &&
					wallE[currX][currY] == UNVISITED)
			{
				if (grid[currX + 1][currY] == TOUCHED ) {
					countUnvisited--;
					wallE[currX][currY] = VISITED;
				} else if (grid[currX][currY] == UNTOUCHED)
					countUnvisited++;
			}

			// SOUTH
			if (currY < MAZEWIDTH - 1 &&
					wallS[currX][currY] == UNVISITED)
			{
				if(grid[currX][currY + 1] == TOUCHED) { 
					countUnvisited--;
					wallS[currX][currY] = VISITED;
				} else if (grid[currX][currY] == UNTOUCHED)
					countUnvisited++;
			}

			// WEST
			if (currX > 0 &&
					wallE[currX - 1][currY] == UNVISITED)
			{
				if (grid[currX - 1][currY] == TOUCHED ) {
					countUnvisited--;
					wallE[currX - 1][currY] = VISITED;
				} else if (grid[currX][currY] == UNTOUCHED)
					countUnvisited++;
			}

//			System.out.println(">" + countUnvisited);
			if(countUnvisited == 0)
				break;

			// mark grid as touched
			grid[currX][currY] = TOUCHED;

			// while there is a direction to go
			while((tempD = findNextDir()) != NODIR) {
				if(goTo(tempD)) {
					moved = true;
					break;
				}
			}
			if(moved) continue;
			else break; // give up
		}

		if (goalFound)
			rover.goal();
		else
			rover.nogoal();
	}

	/**
	 * checkW checks the wall at dirToCheck whether of value valToCheck
	 *
	 */
	boolean checkW(byte dirToCheck, byte valToCheck) {
		byte shiftX = 0, shiftY = 0;

		if (dirToCheck == NORTH) {
			if(currY == 0) 
				return false;
			shiftY = -1;
		} else if (dirToCheck == EAST) {
			if(currX == MAZEWIDTH-1) 
				return false;
		} else if (dirToCheck == SOUTH) {
			if(currY == MAZEWIDTH-1) 
				return false;
		} else if (dirToCheck == WEST) {
			if(currX == 0) 
				return false;
			shiftX = -1;
		}
		if (dirToCheck == NORTH || dirToCheck == SOUTH) {
			return wallS[currX][currY+shiftY] == valToCheck;
		} else// wallE stuff
			return wallE[currX+shiftX][currY] == valToCheck;
	}

	/**
	 * goTo moves in the dirToCheck direction and performs necessary
	 * actions to wallS and wallE depending on result of move
	 *
	 */
	boolean goTo(byte dirToCheck)
		throws InterruptedException
	{
		byte shiftX = 0, shiftY = 0, posShiftX = 0, posShiftY = 0;

		if (dirToCheck == NORTH) {
			if(currY == 0) 
				return false;
			shiftY = -1;
			posShiftY = -1;
		} else if (dirToCheck == EAST) {
			if(currX == MAZEWIDTH-1) 
				return false;
			posShiftX = 1; // +1
		} else if (dirToCheck == SOUTH) {
			if(currY == MAZEWIDTH-1) 
				return false;
			posShiftY = 1; // +1
		} else if (dirToCheck == WEST) {
			if(currX == 0) 
				return false;
			shiftX = -1;
			posShiftX = -1;
		}

		if (dirToCheck == NORTH || dirToCheck == SOUTH) {
			if(wallS[currX][currY+shiftY] != VISITED) {
				// turn to the desired direction
				turnTo(dirToCheck);

				if (rover.forward(1)) { // success of movement
					
					if (wallS[currX][currY+shiftY] == NOWALL)
						wallS[currX][currY + shiftY] = VISITED;
					else
					{
						countUnvisited--;
						wallS[currX][currY + shiftY] = NOWALL;
					}

					currY += posShiftY;
					return true;
				} else { // cannot move forward
						wallS[currX][currY+shiftY] = VISITED;
						countUnvisited--;
					return false;
				}
			}
		} else {// wallE stuff
			if(wallE[currX+shiftX][currY] != VISITED) {
				// turn to desired direction
				turnTo(dirToCheck);
				if (rover.forward(1)) {

					if (wallE[currX+shiftX][currY] == NOWALL)
						wallE[currX + shiftX][currY] = VISITED;
					else {
						countUnvisited--;
						wallE[currX + shiftX][currY] = NOWALL;
					}

					currX += posShiftX;

					return true;
				} else { // cannot move forward
					wallE[currX+shiftX][currY] = VISITED;
					countUnvisited--;
					return false;
				}
			}
		}

		// LOGIC ERROR
		return false;
	}

	void turnTo(byte nextDir) 
	throws InterruptedException
	{
		/** TODO: Make sure that this is rechecked everytime direction
		 *  constants are changed.
		 *
		 */
		// nothing happens when no direction is to be changed
		// clockwise check
		if(nextDir - currDir == 1 || nextDir - currDir == -3) { // WEST to NORTH
			rover.right();
		} else if (currDir - nextDir == 1 || currDir - nextDir == -3) {
			rover.left();
		} else if (currDir - nextDir == 2 || nextDir - currDir == 2) {
			rover.right();
			rover.right();
		}
		currDir = nextDir;
	}

	/**
	 * Pretty complex and unreadable
	 * Basically try UNVISITED first, then NOWALL second
	 * @return The best direction to take to find the goal
	 *
	 */
	byte findNextDir() {

		boolean flag = false;

		for (int i = 0; i < MAZEWIDTH; i++)
			for (int j = 0; j < MAZEWIDTH; j++)
				bfsvisited[i][j] = false;

		bfsState tempcoord = new bfsState();

		bfsvisited[currX][currY] = true;
		q.clear();

		tempD = currDir;
		for (byte turn = 0; turn < 4; turn++) {
			if (checkW(tempD, UNVISITED)) {
				q.addElement(new bfsState(currX + GRIDXOFFSET[tempD], 
							currY + GRIDYOFFSET[tempD], tempD));
				bfsvisited[currX + GRIDXOFFSET[tempD]][currY + GRIDYOFFSET[tempD]] 
					= true;
			}
			tempD++;
			tempD %= 4;
		}

		while (!q.isEmpty()) { 
			// change to .empty() and all q.addElement becomes q.addElement
			tempcoord = (bfsState) q.elementAt(0); q.removeElementAt(0);
			if (tempcoord.x == goalX && tempcoord.y == goalY) {
				flag = true;
				break;
			}

			// NORTH
			if (tempcoord.y > 0 && 
					!bfsvisited[tempcoord.x][tempcoord.y-1] &&
					((wallS[tempcoord.x][tempcoord.y - 1] == UNVISITED && grid[tempcoord.x][tempcoord.y - 1] == UNTOUCHED) ||
					wallS[tempcoord.x][tempcoord.y - 1] == NOWALL))
			{
				q.addElement(new bfsState(tempcoord.x, tempcoord.y-1, tempcoord.dir));
				bfsvisited[tempcoord.x][tempcoord.y-1] = true;
			}
			// EAST
			if (tempcoord.x < MAZEWIDTH - 1 &&
					!bfsvisited[tempcoord.x+1][tempcoord.y] &&
					(wallE[tempcoord.x][tempcoord.y] == NOWALL ||
					(wallE[tempcoord.x][tempcoord.y] == UNVISITED && grid[tempcoord.x+1][tempcoord.y] == UNTOUCHED)))
			{
				q.addElement(new bfsState(tempcoord.x+1, tempcoord.y, tempcoord.dir));
				bfsvisited[tempcoord.x+1][tempcoord.y] = true;
			}
			// SOUTH
			if (tempcoord.y < MAZEWIDTH - 1 &&
					!bfsvisited[tempcoord.x][tempcoord.y + 1] &&
					(wallS[tempcoord.x][tempcoord.y] == NOWALL ||
					(wallS[tempcoord.x][tempcoord.y] == UNVISITED && grid[tempcoord.x][tempcoord.y+1] == UNTOUCHED)))
			{
				q.addElement(new bfsState(tempcoord.x, tempcoord.y+1, tempcoord.dir));
				bfsvisited[tempcoord.x][tempcoord.y+1] = true;
			}
			// WEST
			if (tempcoord.x > 0 &&
					!bfsvisited[tempcoord.x - 1][tempcoord.y] &&
					(wallE[tempcoord.x - 1][tempcoord.y] == NOWALL ||
					(wallE[tempcoord.x - 1][tempcoord.y] == UNVISITED && grid[tempcoord.x-1][tempcoord.y] == UNTOUCHED)))
			{
				q.addElement(new bfsState(tempcoord.x-1, tempcoord.y, tempcoord.dir));
				bfsvisited[tempcoord.x-1][tempcoord.y] = true;
			}
		}

		if (flag)
			return tempcoord.dir;
		else {

			tempD = currDir;
			for (byte turn = 0; turn < 4; turn++) {
				if (checkW(tempD, NOWALL)) {
					q.addElement(new bfsState(currX + GRIDXOFFSET[tempD], 
								currY + GRIDYOFFSET[tempD], tempD));
					bfsvisited[currX + GRIDXOFFSET[tempD]][currY + GRIDYOFFSET[tempD]] 
						= true;
				}
				tempD++;
				tempD %= 4;
			}

			for (int i = 0; i < MAZEWIDTH; i++)
				for (int j = 0; j < MAZEWIDTH; j++)
					bfsvisited[i][j] = false;

			while (!q.isEmpty()) { 
				// change to .empty() and all q.addElement becomes q.addElement
				tempcoord = (bfsState) q.elementAt(0); q.removeElementAt(0);
				if (tempcoord.x == goalX && tempcoord.y == goalY) {
					flag = true;
					break;
				}

				// NORTH
				if (tempcoord.y > 0 && 
						!bfsvisited[tempcoord.x][tempcoord.y-1] &&
						wallS[tempcoord.x][tempcoord.y - 1] != VISITED)
				{
					q.addElement(new bfsState(tempcoord.x, tempcoord.y-1, tempcoord.dir));
					bfsvisited[tempcoord.x][tempcoord.y-1] = true;
				}
				// EAST
				if (tempcoord.x < MAZEWIDTH - 1 &&
						!bfsvisited[tempcoord.x+1][tempcoord.y] &&
						wallE[tempcoord.x][tempcoord.y] != VISITED)
				{
					q.addElement(new bfsState(tempcoord.x+1, tempcoord.y, tempcoord.dir));
					bfsvisited[tempcoord.x+1][tempcoord.y] = true;
				}
				// SOUTH
				if (tempcoord.y < MAZEWIDTH - 1 &&
						!bfsvisited[tempcoord.x][tempcoord.y+1] &&
						wallS[tempcoord.x][tempcoord.y] != VISITED)
				{
					q.addElement(new bfsState(tempcoord.x, tempcoord.y+1, tempcoord.dir));
					bfsvisited[tempcoord.x][tempcoord.y+1] = true;
				}
				// WEST
				if (tempcoord.x > 0 &&
						!bfsvisited[tempcoord.x-1][tempcoord.y] &&
						wallE[tempcoord.x - 1][tempcoord.y] != VISITED)
				{
					q.addElement(new bfsState(tempcoord.x-1, tempcoord.y, tempcoord.dir));
					bfsvisited[tempcoord.x-1][tempcoord.y] = true;
				}
			}

			if (flag)
				return tempcoord.dir;
			else
				return NODIR;
		}
	}

	public static void main(String arg[])
	throws InterruptedException {
		MySuperRobot robot = new MySuperRobot();
		robot.move();
	}

	class bfsState {
		public byte x, y, dir;
		public bfsState() {}
		public bfsState(int a, int b, int c) 
		{ x = (byte) a; y = (byte) b; dir = (byte) c;}
	}
}
